@******************** (C) COPYRIGHT 2009  STMicroelectronics ********************
@* File Name          : cr4_fft_256_stm32.s
@* Author             : MCD Application Team
@* Version            : V2.0.0
@* Date               : 04/27/2009
@* Description        : Optimized 256-point radix-4 complex FFT for Cortex-M3
@*******************************************************************************
@* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
@* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
@* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
@* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
@* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
@* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
@******************************************************************************/

    .thumb
    .syntax unified

    .section .text, "ax", %progbits
    .align 2

    .global cr4_fft_256_stm32
    .extern TableFFT

pssK      .req r0
pssOUT    .req r0
pssX      .req r1
pssIN     .req r1
butternbr .req r2
Nbin      .req r2
index     .req r3
Ar        .req r4
Ai        .req r5
Br        .req r6
Bi        .req r7
Cr        .req r8
Ci        .req r9
Dr        .req r10
Di        .req r11
cntrbitrev .req r12
tmp       .req r12
pssIN2    .req r14
tmp2      .req r14

.set NPT, 256

@----------------------------- MACROS ----------------------------------------
    .macro DEC reg
    sub \reg, \reg, #1
    .endm

    .macro INC reg
    add \reg, \reg, #1
    .endm

    .macro QUAD reg
    lsl \reg, \reg, #2
    .endm

@ sXi = *(PssX+1); sXr = *PssX; PssX += offset; PssX= R1
    .macro LDR2Q sXr, sXi, PssX, offset
    ldrsh \sXi, [\PssX, #2]
    ldrsh \sXr, [\PssX]
    add \PssX, \PssX, \offset
    .endm

@ !! Same macro, to be used when passing negative offset value !!
    .macro LDR2Qm sXr, sXi, PssX, offset
    ldrsh \sXi, [\PssX, #2]
    ldrsh \sXr, [\PssX]
    sub \PssX, \PssX, \offset
    .endm

@ (PssX+1)= sXi;  *PssX=sXr; PssX += offset;
    .macro STR2Q sXr, sXi, PssX, offset
    strh \sXi, [\PssX, #2]
    strh \sXr, [\PssX]
    add \PssX, \PssX, \offset
    .endm

@ YY = Cplx_conjugate_mul(Y,K)
@  Y = YYr + i*YYi
@ use the following trick
@  K = (Kr-Ki) + i*Ki
    .macro CXMUL_V7 YYr, YYi, Yr, Yi, Kr, Ki, tmp, tmp2
    sub \tmp2, \Yi, \Yr         @ sYi-sYr
    mul \tmp, \tmp2, \Ki        @ (sYi-sYr)*sKi
    add \tmp2, \Kr, \Ki, lsl #1 @ (sKr+sKi)
    mla \YYi, \Yi, \Kr, \tmp    @ lYYi = sYi*sKr-sYr*sKi
    mla \YYr, \Yr, \tmp2, \tmp  @ lYYr = sYr*sKr+sYi*sKi
    .endm

@ Four point complex Fast Fourier Transform
    .macro CXADDA4 s
    @ (C,D) = (C+D, C-D)
    add Cr, Cr, Dr
    add Ci, Ci, Di
    sub Dr, Cr, Dr, lsl #1
    sub Di, Ci, Di, lsl #1
    @ (A,B) = (A+(B>>s), A-(B>>s))/4
    mov Ar, Ar, asr #2
    mov Ai, Ai, asr #2
    add Ar, Ar, Br, asr #(2+\s)
    add Ai, Ai, Bi, asr #(2+\s)
    sub Br, Ar, Br, asr #(1+\s)
    sub Bi, Ai, Bi, asr #(1+\s)
    @ (A,C) = (A+(C>>s)/4, A-(C>>s)/4)
    add Ar, Ar, Cr, asr #(2+\s)
    add Ai, Ai, Ci, asr #(2+\s)
    sub Cr, Ar, Cr, asr #(1+\s)
    sub Ci, Ai, Ci, asr #(1+\s)
    @ (B,D) = (B-i*(D>>s)/4, B+i*(D>>s)/4)
    add Br, Br, Di, asr #(2+\s)
    sub Bi, Bi, Dr, asr #(2+\s)
    sub Di, Br, Di, asr #(1+\s)
    add Dr, Bi, Dr, asr #(1+\s)
    .endm

    .macro BUTFLY4ZERO_OPT pIN, offset, pOUT
    ldrsh Ai, [\pIN, #2]
    ldrsh Ar, [\pIN]
    add \pIN, #NPT
    ldrsh Ci, [\pIN, #2]
    ldrsh Cr, [\pIN]
    add \pIN, #NPT
    ldrsh Bi, [\pIN, #2]
    ldrsh Br, [\pIN]
    add \pIN, #NPT
    ldrsh Di, [\pIN, #2]
    ldrsh Dr, [\pIN]
    add \pIN, #NPT

    @ (C,D) = (C+D, C-D)
    add Cr, Cr, Dr
    add Ci, Ci, Di
    sub Dr, Cr, Dr, lsl #1  @ trick
    sub Di, Ci, Di, lsl #1  @ trick
    @ (A,B) = (A+B)/4, (A-B)/4
    mov Ar, Ar, asr #2
    mov Ai, Ai, asr #2
    add Ar, Ar, Br, asr #2
    add Ai, Ai, Bi, asr #2
    sub Br, Ar, Br, asr #1
    sub Bi, Ai, Bi, asr #1
    @ (A,C) = (A+C)/4, (A-C)/4
    add Ar, Ar, Cr, asr #2
    add Ai, Ai, Ci, asr #2
    sub Cr, Ar, Cr, asr #1
    sub Ci, Ai, Ci, asr #1
    @ (B,D) = (B-i*D)/4, (B+i*D)/4
    add Br, Br, Di, asr #2
    sub Bi, Bi, Dr, asr #2
    sub Di, Br, Di, asr #1
    add Dr, Bi, Dr, asr #1
    @
    strh Ai, [\pOUT, #2]
    strh Ar, [\pOUT], #4
    strh Bi, [\pOUT, #2]
    strh Br, [\pOUT], #4
    strh Ci, [\pOUT, #2]
    strh Cr, [\pOUT], #4
    strh Dr, [\pOUT, #2]  @ inversion here
    strh Di, [\pOUT], #4
    .endm

    .macro BUTFLY4_V7 pssDin, offset, pssDout, qformat, pssK
    LDR2Qm Ar, Ai, \pssDin, \offset @ -$offset
    LDR2Q Dr, Di, \pssK, #4
    @ format CXMUL_V7 YYr, YYi, Yr, Yi, Kr, Ki, tmp, tmp2
    CXMUL_V7 Dr, Di, Ar, Ai, Dr, Di, tmp, tmp2
    LDR2Qm Ar, Ai, \pssDin, \offset @ -$offset
    LDR2Q Cr, Ci, \pssK, #4
    CXMUL_V7 Cr, Ci, Ar, Ai, Cr, Ci, tmp, tmp2
    LDR2Qm Ar, Ai, \pssDin, \offset @ -$offset
    LDR2Q Br, Bi, \pssK, #4
    CXMUL_V7 Br, Bi, Ar, Ai, Br, Bi, tmp, tmp2
    LDR2Q Ar, Ai, \pssDin, #0
    CXADDA4 \qformat
    strh Ai, [\pssDout, #2]
    strh Ar, [\pssDout]
    add \pssDout, \pssDout, \offset
    strh Bi, [\pssDout, #2]
    strh Br, [\pssDout]
    add \pssDout, \pssDout, \offset
    strh Ci, [\pssDout, #2]
    strh Cr, [\pssDout]
    add \pssDout, \pssDout, \offset
    strh Dr, [\pssDout, #2]  @ inversion here
    strh Di, [\pssDout], #4
    .endm

@------------------- CODE ---------------------------------
@===============================================================================
@*******************************************************************************
@* Function Name  : cr4_fft_256_stm32
@* Description    : complex radix-4 256 points FFT
@* Input          : - R0 = pssOUT: Output array .
@*                  - R1 = pssIN: Input array
@*                  - R2 = Nbin: =256 number of points, this optimized FFT function
@*                    can only convert 256 points.
@* Output         : None
@* Return         : None
@*******************************************************************************
cr4_fft_256_stm32:
    push {r4-r11, lr}

    mov cntrbitrev, #0
    mov index, #0

preloop_v7:
    add pssIN2, pssIN, cntrbitrev, lsr #24 @ 256-pts
    BUTFLY4ZERO_OPT pssIN2, Nbin, pssOUT
    INC index
    rbit cntrbitrev, index
    cmp index, #64  @ 256-pts
    bne preloop_v7

    sub pssX, pssOUT, Nbin, lsl #2
    mov index, #16
    movs butternbr, Nbin, lsr #4   @ dual use of register

@------------------------------------------------------------------------------
@   The FFT coefficients table can be stored into Flash or RAM.
@   The following two lines of code allow selecting the method for coefficients
@   storage.
@   In the case of choosing coefficients in RAM, you have to:
@   1. Include the file table_fft.h, which is a part of the DSP library,
@      in your main file.
@   2. Decomment the line LDR.W pssK, =TableFFT and comment the line
@      ADRL    pssK, TableFFT_V7
@   3. Comment all the TableFFT_V7 data.
@------------------------------------------------------------------------------
    adr pssK, TableFFT_V7    @ Coeff in Flash
    @ ldr pssK, =TableFFT      @ Coeff in RAM

@................................
passloop_v7:
    push {pssX, butternbr}
    add tmp, index, index, lsl #1
    add pssX, pssX, tmp
    sub butternbr, butternbr, #1<<16
@................
grouploop_v7:
    add butternbr, butternbr, index, lsl #(16-2)
@.......
butterloop_v7:
    BUTFLY4_V7 pssX, index, pssX, 14, pssK
    subs butternbr, butternbr, #1<<16
    bge butterloop_v7
@.......
    add tmp, index, index, lsl #1
    add pssX, pssX, tmp
    DEC butternbr
    movs tmp2, butternbr, lsl #16
    it ne
    subne pssK, pssK, tmp
    bne grouploop_v7
@................
    pop {pssX, butternbr}
    QUAD index
    movs butternbr, butternbr, lsr #2    @ loop nbr /= radix
    bne passloop_v7
@................................
    pop {r4-r11, pc}

@=============================================================================

TableFFT_V7:
    @ N=16
    .short 0x4000,0x0000, 0x4000,0x0000, 0x4000,0x0000
    .short 0xdd5d,0x3b21, 0x22a3,0x187e, 0x0000,0x2d41
    .short 0xa57e,0x2d41, 0x0000,0x2d41, 0xc000,0x4000
    .short 0xdd5d,0xe782, 0xdd5d,0x3b21, 0xa57e,0x2d41
    @ N=64
    .short 0x4000,0x0000, 0x4000,0x0000, 0x4000,0x0000
    .short 0x2aaa,0x1294, 0x396b,0x0646, 0x3249,0x0c7c
    .short 0x11a8,0x238e, 0x3249,0x0c7c, 0x22a3,0x187e
    .short 0xf721,0x3179, 0x2aaa,0x1294, 0x11a8,0x238e
    .short 0xdd5d,0x3b21, 0x22a3,0x187e, 0x0000,0x2d41
    .short 0xc695,0x3fb1, 0x1a46,0x1e2b, 0xee58,0x3537
    .short 0xb4be,0x3ec5, 0x11a8,0x238e, 0xdd5d,0x3b21
    .short 0xa963,0x3871, 0x08df,0x289a, 0xcdb7,0x3ec5
    .short 0xa57e,0x2d41, 0x0000,0x2d41, 0xc000,0x4000
    .short 0xa963,0x1e2b, 0xf721,0x3179, 0xb4be,0x3ec5
    .short 0xb4be,0x0c7c, 0xee58,0x3537, 0xac61,0x3b21
    .short 0xc695,0xf9ba, 0xe5ba,0x3871, 0xa73b,0x3537
    .short 0xdd5d,0xe782, 0xdd5d,0x3b21, 0xa57e,0x2d41
    .short 0xf721,0xd766, 0xd556,0x3d3f, 0xa73b,0x238e
    .short 0x11a8,0xcac9, 0xcdb7,0x3ec5, 0xac61,0x187e
    .short 0x2aaa,0xc2c1, 0xc695,0x3fb1, 0xb4be,0x0c7c
    @ N=256
    .short 0x4000,0x0000, 0x4000,0x0000, 0x4000,0x0000
    .short 0x3b1e,0x04b5, 0x3e69,0x0192, 0x3cc8,0x0324
    .short 0x35eb,0x0964, 0x3cc8,0x0324, 0x396b,0x0646
    .short 0x306c,0x0e06, 0x3b1e,0x04b5, 0x35eb,0x0964
    .short 0x2aaa,0x1294, 0x396b,0x0646, 0x3249,0x0c7c
    .short 0x24ae,0x1709, 0x37af,0x07d6, 0x2e88,0x0f8d
    .short 0x1e7e,0x1b5d, 0x35eb,0x0964, 0x2aaa,0x1294
    .short 0x1824,0x1f8c, 0x341e,0x0af1, 0x26b3,0x1590
    .short 0x11a8,0x238e, 0x3249,0x0c7c, 0x22a3,0x187e
    .short 0x0b14,0x2760, 0x306c,0x0e06, 0x1e7e,0x1b5d
    .short 0x0471,0x2afb, 0x2e88,0x0f8d, 0x1a46,0x1e2b
    .short 0xfdc7,0x2e5a, 0x2c9d,0x1112, 0x15fe,0x20e7
    .short 0xf721,0x3179, 0x2aaa,0x1294, 0x11a8,0x238e
    .short 0xf087,0x3453, 0x28b2,0x1413, 0x0d48,0x2620
    .short 0xea02,0x36e5, 0x26b3,0x1590, 0x08df,0x289a
    .short 0xe39c,0x392b, 0x24ae,0x1709, 0x0471,0x2afb
    .short 0xdd5d,0x3b21, 0x22a3,0x187e, 0x0000,0x2d41
    .short 0xd74e,0x3cc5, 0x2093,0x19ef, 0xfb8f,0x2f6c
    .short 0xd178,0x3e15, 0x1e7e,0x1b5d, 0xf721,0x3179
    .short 0xcbe2,0x3f0f, 0x1c64,0x1cc6, 0xf2b8,0x3368
    .short 0xc695,0x3fb1, 0x1a46,0x1e2b, 0xee58,0x3537
    .short 0xc197,0x3ffb, 0x1824,0x1f8c, 0xea02,0x36e5
    .short 0xbcf0,0x3fec, 0x15fe,0x20e7, 0xe5ba,0x3871
    .short 0xb8a6,0x3f85, 0x13d5,0x223d, 0xe182,0x39db
    .short 0xb4be,0x3ec5, 0x11a8,0x238e, 0xdd5d,0x3b21
    .short 0xb140,0x3daf, 0x0f79,0x24da, 0xd94d,0x3c42
    .short 0xae2e,0x3c42, 0x0d48,0x2620, 0xd556,0x3d3f
    .short 0xab8e,0x3a82, 0x0b14,0x2760, 0xd178,0x3e15
    .short 0xa963,0x3871, 0x08df,0x289a, 0xcdb7,0x3ec5
    .short 0xa7b1,0x3612, 0x06a9,0x29ce, 0xca15,0x3f4f
    .short 0xa678,0x3368, 0x0471,0x2afb, 0xc695,0x3fb1
    .short 0xa5bc,0x3076, 0x0239,0x2c21, 0xc338,0x3fec
    .short 0xa57e,0x2d41, 0x0000,0x2d41, 0xc000,0x4000
    .short 0xa5bc,0x29ce, 0xfdc7,0x2e5a, 0xbcf0,0x3fec
    .short 0xa678,0x2620, 0xfb8f,0x2f6c, 0xba09,0x3fb1
    .short 0xa7b1,0x223d, 0xf957,0x3076, 0xb74d,0x3f4f
    .short 0xa963,0x1e2b, 0xf721,0x3179, 0xb4be,0x3ec5
    .short 0xab8e,0x19ef, 0xf4ec,0x3274, 0xb25e,0x3e15
    .short 0xae2e,0x1590, 0xf2b8,0x3368, 0xb02d,0x3d3f
    .short 0xb140,0x1112, 0xf087,0x3453, 0xae2e,0x3c42
    .short 0xb4be,0x0c7c, 0xee58,0x3537, 0xac61,0x3b21
    .short 0xb8a6,0x07d6, 0xec2b,0x3612, 0xaac8,0x39db
    .short 0xbcf0,0x0324, 0xea02,0x36e5, 0xa963,0x3871
    .short 0xc197,0xfe6e, 0xe7dc,0x37b0, 0xa834,0x36e5
    .short 0xc695,0xf9ba, 0xe5ba,0x3871, 0xa73b,0x3537
    .short 0xcbe2,0xf50f, 0xe39c,0x392b, 0xa678,0x3368
    .short 0xd178,0xf073, 0xe182,0x39db, 0xa5ed,0x3179
    .short 0xd74e,0xebed, 0xdf6d,0x3a82, 0xa599,0x2f6c
    .short 0xdd5d,0xe782, 0xdd5d,0x3b21, 0xa57e,0x2d41
    .short 0xe39c,0xe33a, 0xdb52,0x3bb6, 0xa599,0x2afb
    .short 0xea02,0xdf19, 0xd94d,0x3c42, 0xa5ed,0x289a
    .short 0xf087,0xdb26, 0xd74e,0x3cc5, 0xa678,0x2620
    .short 0xf721,0xd766, 0xd556,0x3d3f, 0xa73b,0x238e
    .short 0xfdc7,0xd3df, 0xd363,0x3daf, 0xa834,0x20e7
    .short 0x0471,0xd094, 0xd178,0x3e15, 0xa963,0x1e2b
    .short 0x0b14,0xcd8c, 0xcf94,0x3e72, 0xaac8,0x1b5d
    .short 0x11a8,0xcac9, 0xcdb7,0x3ec5, 0xac61,0x187e
    .short 0x1824,0xc850, 0xcbe2,0x3f0f, 0xae2e,0x1590
    .short 0x1e7e,0xc625, 0xca15,0x3f4f, 0xb02d,0x1294
    .short 0x24ae,0xc44a, 0xc851,0x3f85, 0xb25e,0x0f8d
    .short 0x2aaa,0xc2c1, 0xc695,0x3fb1, 0xb4be,0x0c7c
    .short 0x306c,0xc18e, 0xc4e2,0x3fd4, 0xb74d,0x0964
    .short 0x35eb,0xc0b1, 0xc338,0x3fec, 0xba09,0x0646
    .short 0x3b1e,0xc02c, 0xc197,0x3ffb, 0xbcf0,0x0324

    .end
@******************* (C) COPYRIGHT 2009  STMicroelectronics *****END OF FILE****